diff --git a/cpu/sh4/Makefile b/cpu/sh4/Makefile
index b24a280..1bb8bd7 100644
--- a/cpu/sh4/Makefile
+++ b/cpu/sh4/Makefile
@@ -29,7 +29,7 @@
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-OBJS	= cpu.o interrupts.o watchdog.o time.o 
+OBJS	= cpu.o interrupts.o watchdog.o time.o cache.o
 
 all:	.depend $(START) $(LIB)
 
diff --git a/cpu/sh4/cache.c b/cpu/sh4/cache.c
new file mode 100644
index 0000000..55acb31
--- /dev/null
+++ b/cpu/sh4/cache.c
@@ -0,0 +1,108 @@
+/*
+ * (C) Copyright 2007
+ * Nobuhiro Iwamatsu <iwamatsu@nigauri.org>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <command.h>
+#include <asm/processor.h>
+#include <asm/io.h>
+
+/*
+ * Jump to P2 area.
+ * When handling TLB or caches, we need to do it from P2 area.
+ */
+#define jump_to_P2()                    \
+  do {                                    \
+    unsigned long __dummy;		\
+    __asm__ __volatile__(			\
+                "mov.l  1f, %0\n\t"     \
+                "or     %1, %0\n\t"     \
+                "jmp    @%0\n\t"        \
+                " nop\n\t"              \
+                ".balign 4\n"           \
+                "1:     .long 2f\n"     \
+                "2:"                    \
+                : "=&r" (__dummy)       \
+                : "r" (0x20000000));    \
+  } while (0)
+
+/*
+ * Back to P1 area.
+ */
+#define back_to_P1()                                    \
+  do {                                                    \
+    unsigned long __dummy;                          \
+    __asm__ __volatile__(                           \
+                "nop;nop;nop;nop;nop;nop;nop\n\t"       \
+                "mov.l  1f, %0\n\t"                     \
+                "jmp    @%0\n\t"                        \
+                " nop\n\t"                              \
+                ".balign 4\n"                           \
+                "1:     .long 2f\n"                     \
+                "2:"                                    \
+                : "=&r" (__dummy));                     \
+  } while (0)
+
+#define CACHE_VALID       1
+#define CACHE_UPDATED     2
+
+static inline void cache_wback_all(void)
+{
+	unsigned long addr, data, i, j;
+
+	jump_to_P2();
+	for (i = 0; i < CACHE_OC_NUM_ENTRIES; i++){
+		for (j = 0; j < CACHE_OC_NUM_WAYS; j++) {
+      			addr = CACHE_OC_ADDRESS_ARRAY | (j << CACHE_OC_WAY_SHIFT)
+				| (i << CACHE_OC_ENTRY_SHIFT);
+      			data = inl(addr);
+			if (data & CACHE_UPDATED) {
+				data &= ~CACHE_UPDATED;
+				outl(data, addr);
+			}
+		}
+	}
+	back_to_P1();
+}
+
+
+#define CACHE_ENABLE      0
+#define CACHE_DISABLE     1
+
+int cache_control(unsigned int cmd)
+{
+	unsigned long ccr;
+
+	jump_to_P2();
+	ccr = inl(CCR);
+
+	if (ccr & CCR_CACHE_ENABLE)
+		cache_wback_all();
+
+	if (cmd == CACHE_DISABLE)
+		outl(CCR_CACHE_STOP, CCR);
+	else
+		outl(CCR_CACHE_INIT, CCR);
+	back_to_P1();
+
+	return 0;
+}
diff --git a/cpu/sh4/cpu.c b/cpu/sh4/cpu.c
index 70df45e..d76d518 100644
--- a/cpu/sh4/cpu.c
+++ b/cpu/sh4/cpu.c
@@ -23,6 +23,7 @@
 
 #include <common.h>
 #include <command.h>
+#include <asm/processor.h>
 
 int checkcpu(void)
 {
@@ -48,20 +49,35 @@
 	return 0;
 }
 
-void flush_cache (unsigned long addr, unsigned long size){}
+void flush_cache (unsigned long addr, unsigned long size)
+{
+
+}
 
 void icache_enable (void)
 {
-
+	cache_control(0);
 }
 
 void icache_disable (void)
 {
-
+	cache_control(1);
 }
 
-
 int icache_status (void)
 {
 	return 0; 
 }
+
+void dcache_enable (void)
+{
+}
+
+void dcache_disable (void)
+{
+}
+
+int dcache_status (void)
+{
+	return 0;
+}
diff --git a/cpu/sh4/time.c b/cpu/sh4/time.c
index b6e4d02..5f8a3a0 100644
--- a/cpu/sh4/time.c
+++ b/cpu/sh4/time.c
@@ -1,4 +1,7 @@
 /*
+ * (C) Copyright 2007
+ * Nobobuhiro Iwamatsu <iwamatsu@nigauri.org>
+ *
  * (C) Copyright 2003
  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
  *
@@ -31,7 +34,15 @@
 	if (timer > 2)
 		return;
 
-	*((volatile unsigned char *) TSTR0) |= (1 << timer);
+	*((volatile unsigned char *) TSTR) |= (1 << timer);
+}
+
+static void tmu_timer_stop (unsigned int timer)
+{
+	u8 val = *((volatile u8 *)TSTR);
+	if (timer > 2)
+		return;
+	*((volatile unsigned char *)TSTR) = val &~(1 << timer);
 }
 
 int timer_init (void)
@@ -39,7 +50,8 @@
 	/* Divide clock by 4 */
 	*(volatile u16 *)TCR0 = 0;
 
-	tmu_timer_start (0);
+	tmu_timer_stop(0);
+	tmu_timer_start(0);
 	return 0;
 }
 
@@ -51,15 +63,12 @@
 */
 unsigned long long get_ticks (void)
 {
-	return (0 - *((volatile unsigned int *) TCNT0));
+	return (0 - *((volatile u32 *) TCNT0));
 }
 
 unsigned long get_timer (unsigned long base)
 {
-	unsigned long n = 
-		*((volatile unsigned int *)TCNT0) ;
-	
-	return ((int)n - base ) < 0 ? ( TMU_MAX_COUNTER - ( base -n )):(n - base );
+	return ((0 - *((volatile u32 *) TCNT0)) - base);
 }
 
 void set_timer (unsigned long t)
@@ -69,19 +78,15 @@
 
 void reset_timer (void)
 {
+	tmu_timer_stop(0);
 	set_timer (0);
+	tmu_timer_start(0);
 }
 
 void udelay (unsigned long usec)
 {
 	unsigned int start = get_timer (0);
-	unsigned int end = 0;
-	if (usec > 1000000)
-		end = ((usec/100000) * CFG_HZ) / 10;
-	else if (usec > 1000)
-		end = ((usec/100) * CFG_HZ) / 10000;
-	else
-		end = (usec * CFG_HZ) / 1000000;
+	unsigned int end = start + (usec * ((CFG_HZ + 500000) / 1000000));
 
 	while (get_timer (0) < end)
 		continue;
@@ -91,4 +96,3 @@
 {
 	return CFG_HZ;
 }
-
diff --git a/cpu/sh4/watchdog.c b/cpu/sh4/watchdog.c
index e2bc820..04723a7 100644
--- a/cpu/sh4/watchdog.c
+++ b/cpu/sh4/watchdog.c
@@ -32,11 +32,13 @@
 	while (csr_read() & (1 << 5)) {
 		/* delay */
 	}
-	*((volatile unsigned short *)(WDT_BASE + 0x00)) = ((unsigned short) value) | 0x5A00;
+	*((volatile unsigned short *)(WDT_BASE + 0x00)) 
+		= ((unsigned short) value) | 0x5A00;
 }
 
 static void csr_write (unsigned char value){
-	*((volatile unsigned short *)(WDT_BASE + 0x04)) = ((unsigned short) value) | 0xA500;
+	*((volatile unsigned short *)(WDT_BASE + 0x04)) 
+		= ((unsigned short) value) | 0xA500;
 }